 /*
  * $Header: /cvshome/build/org.osgi.util.position/src/org/osgi/util/position/Position.java,v 1.9 2006/07/12 21:21:35 hargrave Exp $
  *
  * Copyright (c) OSGi Alliance (2002, 2006). All Rights Reserved.
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  * http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
 package org.osgi.util.position;

 import org.osgi.util.measurement.Measurement;
 import org.osgi.util.measurement.Unit;

 /**
  * Position represents a geographic location, based on the WGS84 System (World
  * Geodetic System 1984).
  * <p>
  * The <code>org.osgi.util.measurement.Measurement</code> class is used to
  * represent the values that make up a position.
  * <p>
  * <p>
  * A given position object may lack any of it's components, i.e. the altitude
  * may not be known. Such missing values will be represented by null.
  * <p>
  * Position does not override the implementation of either equals() or
  * hashCode() because it is not clear how missing values should be handled. It
  * is up to the user of a position to determine how best to compare two position
  * objects. A <code>Position</code> object is immutable.
  */
 public class Position {
     private Measurement altitude;
     private Measurement longitude;
     private Measurement latitude;
     private Measurement speed;
     private Measurement track;

     /**
      * Contructs a <code>Position</code> object with the given values.
      *
      * @param lat a <code>Measurement</code> object specifying the latitude in
      * radians, or null
      * @param lon a <code>Measurement</code> object specifying the longitude in
      * radians, or null
      * @param alt a <code>Measurement</code> object specifying the altitude in
      * meters, or null
      * @param speed a <code>Measurement</code> object specifying the speed in
      * meters per second, or null
      * @param track a <code>Measurement</code> object specifying the track in
      * radians, or null
      */
     public Position(Measurement lat, Measurement lon, Measurement alt,
             Measurement speed, Measurement track) {
         if (lat != null) {
             if (!Unit.rad.equals(lat.getUnit())) {
                 throw new IllegalArgumentException ("Invalid Latitude");
             }
             this.latitude = lat;
         }
         if (lon != null) {
             if (!Unit.rad.equals(lon.getUnit())) {
                 throw new IllegalArgumentException ("Invalid Longitude");
             }
             this.longitude = lon;
         }
         normalizeLatLon();
         if (alt != null) {
             if (!Unit.m.equals(alt.getUnit())) {
                 throw new IllegalArgumentException ("Invalid Altitude");
             }
             this.altitude = alt;
         }
         if (speed != null) {
             if (!Unit.m_s.equals(speed.getUnit())) {
                 throw new IllegalArgumentException ("Invalid Speed");
             }
             this.speed = speed;
         }
         if (track != null) {
             if (!Unit.rad.equals(track.getUnit())) {
                 throw new IllegalArgumentException ("Invalid Track");
             }
             this.track = normalizeTrack(track);
         }
     }

     /**
      * Returns the altitude of this position in meters.
      *
      * @return a <code>Measurement</code> object in <code>Unit.m</code> representing
      * the altitude in meters above the ellipsoid <code>null</code> if the
      * altitude is not known.
      */
     public Measurement getAltitude() {
         return altitude;
     }

     /**
      * Returns the longitude of this position in radians.
      *
      * @return a <code>Measurement</code> object in <code>Unit.rad</code>
      * representing the longitude, or <code>null</code> if the longitude
      * is not known.
      */
     public Measurement getLongitude() {
         return longitude;
     }

     /**
      * Returns the latitude of this position in radians.
      *
      * @return a <code>Measurement</code> object in <code>Unit.rad</code>
      * representing the latitude, or <code>null</code> if the latitude is
      * not known..
      */
     public Measurement getLatitude() {
         return latitude;
     }

     /**
      * Returns the ground speed of this position in meters per second.
      *
      * @return a <code>Measurement</code> object in <code>Unit.m_s</code>
      * representing the speed, or <code>null</code> if the speed is not
      * known..
      */
     public Measurement getSpeed() {
         return speed;
     }

     /**
      * Returns the track of this position in radians as a compass heading. The
      * track is the extrapolation of previous previously measured positions to a
      * future position.
      *
      * @return a <code>Measurement</code> object in <code>Unit.rad</code>
      * representing the track, or <code>null</code> if the track is not
      * known..
      */
     public Measurement getTrack() {
         return track;
     }

     private static final double LON_RANGE = Math.PI;
     private static final double LAT_RANGE = Math.PI / 2.0D;

     /**
      * Verify the longitude and latitude parameters so they fit the normal
      * coordinate system. A latitude is between -90 (south) and +90 (north). A A
      * longitude is between -180 (Western hemisphere) and +180 (eastern
      * hemisphere). This method first normalizes the latitude and longitude
      * between +/- 180. If the |latitude| > 90, then the longitude is added 180
      * and the latitude is normalized to fit +/-90. (Example are with degrees
      * though radians are used) <br>
      * No normalization takes place when either lon or lat is null.
      */
     private void normalizeLatLon() {
         if (longitude == null || latitude == null)
             return;
         double dlon = longitude.getValue();
         double dlat = latitude.getValue();
         if (dlon >= -LON_RANGE && dlon < LON_RANGE && dlat >= -LAT_RANGE
                 && dlat <= LAT_RANGE)
             return;
         dlon = normalize(dlon, LON_RANGE);
         dlat = normalize(dlat, LAT_RANGE * 2.0D); // First over 180 degree
 // Check if we have to move to other side of the earth
 if (dlat > LAT_RANGE || dlat < -LAT_RANGE) {
             dlon = normalize(dlon - LON_RANGE, LON_RANGE);
             dlat = normalize((LAT_RANGE * 2.0D) - dlat, LAT_RANGE);
         }
         longitude = new Measurement(dlon, longitude.getError(), longitude
                 .getUnit(), longitude.getTime());
         latitude = new Measurement(dlat, latitude.getError(), latitude
                 .getUnit(), latitude.getTime());
     }

     /**
      * This function normalizes the a value according to a range. This is not
      * simple modulo (as I thought when I started), but requires some special
      * handling. For positive numbers we subtract 2*range from the number so
      * that end up between -/+ range. For negative numbers we add this value.
      * For example, if the value is 270 and the range is +/- 180. Then sign=1 so
      * the (int) factor becomes 270+180/360 = 1. This means that 270-360=-90 is
      * the result. (degrees are only used to make it easier to understand, this
      * function is agnostic for radians/degrees). The result will be in
      * [range,range&gt; The algorithm is not very fast, but it handling the
      * [&gt; ranges made it very messy using integer arithmetic, and this is
      * very readable. Note that it is highly unlikely that this method is called
      * in normal situations. Normally input values to position are already
      * normalized because they come from a GPS. And this is much more readable.
      *
      * @param value The value that needs adjusting
      * @param range -range = < value < range
      */
     private double normalize(double value, double range) {
         double twiceRange = 2.0D * range;
         while (value >= range) {
             value -= twiceRange;
         }
         while (value < -range) {
             value += twiceRange;
         }
         return value;
     }

     private static final double TRACK_RANGE = Math.PI * 2.0D;

     /**
      * Normalize track to be a value such that: 0 <= value < +2PI. This
      * corresponds to 0 deg to +360 deg. 0 is North 0.5*PI is East PI is South
      * 1.5*PI is West
      *
      * @param track Value to be normalized
      * @return Normalized value
      */
     private Measurement normalizeTrack(Measurement track) {
         double value = track.getValue();
         if ((0.0D <= value) && (value < TRACK_RANGE)) {
             return track; /* value is already normalized */
         }
         value %= TRACK_RANGE;
         if (value < 0.0D) {
             value += TRACK_RANGE;
         }
         return new Measurement(value, track.getError(), track.getUnit(), track
                 .getTime());
     }
 }

